#include "special_io.h"
#include <iostream>
#include <map>

SpecialIo::SpecialIo(std::shared_ptr<rclcpp::Node>& node) : 
    node_(node),
    m_init_ok(false)
{
    m_ioControl = std::make_shared<OutPutIoControl>(node_);
    
    m_pose_sub = node_->create_subscription<agv_msgs::msg::PoseWithConfidence>(
        "pose", 1, 
        [this](const agv_msgs::msg::PoseWithConfidence::SharedPtr msg) { poseCB(msg); });
    
    m_network_connection_sub = node_->create_subscription<agv_msgs::msg::ConnectionMonitor>(
        "connection_state", 1,
        [this](const agv_msgs::msg::ConnectionMonitor::SharedPtr msg) { networkCB(msg); });
    
    m_event_sub = node_->create_subscription<agv_msgs::msg::AGVEventStatus>(
        "/eventstatus", 1,
        [this](const agv_msgs::msg::AGVEventStatus::SharedPtr msg) { eventCB(msg); });
    
    m_battery_sub = node_->create_subscription<agv_msgs::msg::BatInfo>(
        "/batteryInfo", 1,
        [this](const agv_msgs::msg::BatInfo::SharedPtr msg) { batteryCB(msg); });

    m_quit_mutex.lock();
    m_thread = std::thread(&SpecialIo::thread_fun, this);
    m_init_ok = true;
}

SpecialIo::~SpecialIo()
{
    if(m_quit_mutex.try_lock()) {
        m_quit_mutex.unlock();
    }
    if(m_thread.joinable()) {
        m_thread.join();
    }
}

void SpecialIo::cycle()
{
    static auto lastTime = node_->now();
    auto diff_time = (node_->now() - lastTime).seconds() * 1000;
    
    if (diff_time > 1000) {
        lastTime = node_->now();

        // #2 网络在线输出灯
        bool disconnection = m_msg_to_io.netMsg.brokerconnnection || m_msg_to_io.netMsg.masterconnnection;
        m_ioControl->netConnectLight.set(!disconnection);

        // #3 低电量灯 小于25%
        m_ioControl->lowBatteryLight.set(m_msg_to_io.batMsg.numeric_bat < 25);

        // #4 置信度高于15成功
        m_ioControl->locationLight.set(m_msg_to_io.poseMsg.score > 15.0);

        // #5 需要复位红色灯
        m_ioControl->emgLight.set(m_msg_to_io.eventMsg.ecode != 0);
    }
}


void SpecialIo::thread_fun(SpecialIo *pThis)
{
    if (!pThis)
        return;

    while (!pThis->m_quit_mutex.try_lock_for(std::chrono::milliseconds(100)))
    {
        pThis->cycle();
    }
}